#pragma once
#include "../../Utility/Util/utility.hpp"
#include "../../Utility/Data/NavMessageData/GNSSMesData.hpp"
#include "../../Utility/Data/NavObsData/GNSSObsData.hpp"
#include "../../Utility/Data/Position/CRDFactory.hpp"
#include "../../Utility/Data/Time/TimeFactory.hpp"
#include "../../Utility/Log/Log.hpp"
#include "../NavCalculate/NavCalculateResult.hpp"
#include "../IonoCorrect/IonoCorrect.hpp"
#include "../TropoCorrect/TropoCorrect.hpp"

namespace ns_Calculate
{
    using namespace ns_Log;
    using namespace ns_Util;
    using namespace ns_GNSSMesData;
    using namespace ns_GNSSObsData;

    class SinglePointCalculate
    {
    public:
        SinglePointCalculate()
        {
        }

        static bool Postion(const std::vector<GNSSObsRecord::Ptr> &obsDatas, const GNSSObsHeader::Ptr &obsHeader,
                            const std::vector<GNSSMesRecord::Ptr> &mesDatas, const GNSSMesHeader::Ptr &mesHeader,
                            CRDFactory::CRDPtr *output, Matrix *DOP)
        {
            if (output == nullptr)
            {
                Log(Debug) << "output为空" << '\n';
                return false;
            }

            int obsDataSize = obsDatas.size();

            if (obsDataSize <= 4)
            {
                Log(Debug) << "样本数量不足" << '\n';
                return false;
            }

            std::unordered_map<GNSSType, std::unordered_map<int, GNSSMesRecord::Ptr>> hash;

            for (const auto &e : mesDatas)
            {
                if (e == nullptr)
                    continue;

                hash[e->gnssType][e->serial] = e;
            }

            GNSSObsRecord::GNSSObsRecordHeader::Ptr epochHeader = obsDatas[0]->header;
            if (epochHeader == nullptr)
            {
                Log(Debug) << "历元头为空" << '\n';
                return false;
            }

            std::vector<double> reciverInitPositionVector(3);
            reciverInitPositionVector[0] = -2267749.0000;
            reciverInitPositionVector[1] = 5009154.0000;
            reciverInitPositionVector[2] = 3221290.0000;
            if (obsHeader != nullptr)
            {
                reciverInitPositionVector[0] = obsHeader->approxPositon[0];
                reciverInitPositionVector[1] = obsHeader->approxPositon[1];
                reciverInitPositionVector[2] = obsHeader->approxPositon[2];
            }

            // 设计矩阵
            Matrix X(Matrix::MatrixContainerType({{reciverInitPositionVector[0]},
                                                  {reciverInitPositionVector[1]},
                                                  {reciverInitPositionVector[2]},
                                                  {epochHeader->reciverClockBias}}));

            int Count = 0;
            while (true)
            {
                auto reciverPosition = CRDFactory::MakeCRDPtr(CRDFactory::ValueType::CartesianCRDSys,
                                                              X[0][0], X[1][0], X[2][0]);

                Matrix B(obsDataSize, 4);
                Matrix W(obsDataSize, 1);
                Matrix P(obsDataSize, obsDataSize);

                // 计算每颗卫星
                for (int i = 0; i < obsDataSize; i++)
                {
                    GNSSObsRecord::Ptr obsRecord = obsDatas[i];
                    GNSSMesRecord::Ptr mesRecord = hash[obsRecord->gnssType][obsRecord->serial];
                    if (mesRecord == nullptr)
                    {
                        Log(Debug) << "没有对应的卫星数据" << '\n';
                        continue;
                    }

                    double pseudoRange = obsRecord->pseudoRange;
                    auto curTime = TimeFactory::MakeTimePtr(TimeFactory::ValueType::CommonTimeSys, epochHeader->epoch);

                    auto mesData = std::make_shared<GNSSMesData>(mesHeader, mesRecord, mesRecord->gnssType);

                    // 计算卫星的位置和速度
                    // 方法：以信号传播时间判断是否收敛，如果信号传播时间收敛了，则可知道卫星位置和速度
                    double rrClockBias = X[3][0];

                    // 信号传播时间，初值设为0.075
                    double signalPropagationTime = 0.0075;

                    int count = 0;
                    while (true)
                    {
                        // 在循环里.不断计算信号传播时间，一直到信号传播时间收敛
                        double second = curTime->SecondFromRefTime() - signalPropagationTime;
                        auto _signalPostTime = TimeFactory::MakeTimePtr(TimeFactory::ValueType::UnixTimeSys)->GetRefTime()->TimeAfterSecond(second);
                        auto signalPostTime = std::shared_ptr<TimeSys>(_signalPostTime);

                        NavCalculateResult navCal(mesData, signalPostTime);
                        auto rsPosition = navCal.GetPosition();
                        auto rsClockBias = navCal.GetClockBias();

                        double distance = DistanceOfCoordinate(reciverPosition, rsPosition);

                        // 新的信号传播时间为 距离/光速 - 接收机钟差 + 卫星钟差
                        double newsignalPropagationTime = distance / Mathf::c - rrClockBias / Mathf::c + rsClockBias;

                        if (Mathf::Abs(newsignalPropagationTime - signalPropagationTime) < 1e-7)
                            break;

                        if (count > 10)
                        {
                            Log(Debug) << "迭代次数为：" << count << '\n'
                                       << "未找到收敛的信号传播时间" << '\n';
                            break;
                        }

                        signalPropagationTime = newsignalPropagationTime;
                        count++;
                    }

                    // 找到了信号传播时间之后，确定了卫星坐标，开始填充矩阵
                    // 在循环里.不断计算信号传播时间，一直到信号传播时间收敛
                    double second = curTime->SecondFromRefTime() - signalPropagationTime;
                    auto _signalPostTime = TimeFactory::MakeTimePtr(TimeFactory::ValueType::UnixTimeSys)->GetRefTime()->TimeAfterSecond(second);
                    auto signalPostTime = std::shared_ptr<TimeSys>(_signalPostTime);
                    NavCalculateResult navCal(mesData, signalPostTime);
                    auto _rsPosition = navCal.GetPosition();
                    auto rsPosition = std::dynamic_pointer_cast<CartesianCRD>(_rsPosition);

                    auto rrPosition = std::dynamic_pointer_cast<CartesianCRD>(reciverPosition);

                    double distance = DistanceOfCoordinate(rsPosition, rrPosition);
                    double l = (rsPosition->GetX() - rrPosition->GetX()) / distance;

                    double m = (rsPosition->GetY() - rrPosition->GetY()) / distance;

                    double n = (rsPosition->GetZ() - rrPosition->GetZ()) / distance;

                    double ionoCorr = IonoCorrect::GetKlobuchar(obsRecord->gnssType, rrPosition, rsPosition, mesHeader, curTime);

                    double tropoCorr = TropoCorrect::GetHopfield(rrPosition, rsPosition, curTime);

                    double w = pseudoRange - (distance + rrClockBias - Mathf::c * navCal.GetClockBias() + ionoCorr + tropoCorr);

                    auto tmp = CRDFactory::MakeCRDPtr(CRDFactory::ValueType::TopoCentricPolarCRDSys, *rrPosition);

                    CRDFactory::SwitchTo(CRDFactory::ValueType::TopoCentricPolarCRDSys, rsPosition, &tmp, rrPosition);
                    std::shared_ptr<TopoCentricPolarCRD> ans = std::dynamic_pointer_cast<TopoCentricPolarCRD>(tmp);
                    double p = Mathf::Sin(ans->GetElevation()) * Mathf::Sin(ans->GetElevation());

                    B[i][0] = l;
                    B[i][1] = m;
                    B[i][2] = n;
                    B[i][3] = 1;

                    W[i][0] = w;

                    P[i][i] = p;
                }

                // 矩阵填充完毕，开始检验结果
                Matrix x = Matrixf::InverseMatrix(Matrixf::TransposeMatrix(B) * P * B) * Matrixf::TransposeMatrix(B) * P * W;
                x.Normalize();

                X[0][0] += x[0][0];
                X[1][0] += x[1][0];
                X[2][0] += x[2][0];
                X[3][0] += x[3][0];

                if (Count >= 7)
                {
                    Log(Debug) << "定位失败" << '\n';
                }

                if ((Mathf::Abs(x[0][0]) < 0.001 && Mathf::Abs(x[1][0]) < 0.001 && Mathf::Abs(x[2][0]) < 0.001) || Count >= 7)
                {
                    Matrix V = B * X - Matrixf::MulMatrix(Mathf::c, W);
                    double sigma = Mathf::Sqrt((Matrixf::TransposeMatrix(V) * P * V)[0][0] / (obsDataSize - 4));
                    Log(Debug) << sigma << '\n';
                    Matrix D = Matrixf::MulMatrix(sigma, Matrixf::InverseMatrix(Matrixf::TransposeMatrix(B) * P * B));
                    D.Normalize();
                    if (DOP != nullptr)
                        *DOP = D;

                    break;
                }

                Count++;
            }

            *output = CRDFactory::MakeCRDPtr(CRDFactory::ValueType::CartesianCRDSys, X[0][0], X[1][0], X[2][0]);

            return true;
        }

        static bool Velocity(const std::vector<GNSSObsRecord::Ptr> &obsDatas, const GNSSObsHeader::Ptr &obsHeader,
                             const std::vector<GNSSMesRecord::Ptr> &mesDatas, const GNSSMesHeader::Ptr &mesHeader,
                             std::vector<double> *output, Matrix *DOP)
        {
            if (output == nullptr)
            {
                Log(Debug) << "output为空" << '\n';
                return false;
            }

            int obsDataSize = obsDatas.size();

            if (obsDataSize <= 4)
            {
                Log(Debug) << "样本数量不足" << '\n';
                return false;
            }

            std::unordered_map<GNSSType, std::unordered_map<int, GNSSMesRecord::Ptr>> hash;

            for (const auto &e : mesDatas)
            {
                if (e == nullptr)
                    continue;

                hash[e->gnssType][e->serial] = e;
            }

            GNSSObsRecord::GNSSObsRecordHeader::Ptr epochHeader = obsDatas[0]->header;
            if (epochHeader == nullptr)
            {
                Log(Debug) << "历元头为空" << '\n';
                return false;
            }
            CRDFactory::CRDPtr reciverPosition = CRDFactory::MakeCRDPtr(CRDFactory::ValueType::CartesianCRDSys);
            Postion(obsDatas, obsHeader, mesDatas, mesHeader, &reciverPosition, nullptr);

            // 构建矩阵
            Matrix B(obsDataSize, 4);
            Matrix W(obsDataSize, 1);
            Matrix P(obsDataSize, obsDataSize);
            for (int i = 0; i < obsDataSize; i++)
            {
                GNSSObsRecord::Ptr obsRecord = obsDatas[i];
                GNSSMesRecord::Ptr mesRecord = hash[obsRecord->gnssType][obsRecord->serial];
                if (mesRecord == nullptr)
                {
                    Log(Debug) << "没有对应的卫星数据" << '\n';
                    continue;
                }

                double doppler = obsRecord->doppler;
                auto curTime = TimeFactory::MakeTimePtr(TimeFactory::ValueType::CommonTimeSys, epochHeader->epoch);

                auto mesData = std::make_shared<GNSSMesData>(mesHeader, mesRecord, mesRecord->gnssType);
                // 信号传播时间，初值设为0.75
                double signalPropagationTime = 0.075;

                int count = 0;
                while (true)
                {
                    // 在循环里.不断计算信号传播时间，一直到信号传播时间收敛
                    double second = curTime->SecondFromRefTime() - signalPropagationTime;
                    auto _signalPostTime = TimeFactory::MakeTimePtr(TimeFactory::ValueType::UnixTimeSys)->GetRefTime()->TimeAfterSecond(second);
                    auto signalPostTime = std::shared_ptr<TimeSys>(_signalPostTime);

                    NavCalculateResult navCal(mesData, signalPostTime);
                    auto rsPosition = navCal.GetPosition();
                    auto rsClockBias = navCal.GetClockBias();

                    double distance = DistanceOfCoordinate(reciverPosition, rsPosition);

                    // 新的信号传播时间为 距离/光速 + 卫星钟差
                    double newsignalPropagationTime = distance / Mathf::c + rsClockBias;

                    if (Mathf::Abs(newsignalPropagationTime - signalPropagationTime) < 1e-7)
                    {
                        break;
                    }

                    if (count > 10)
                    {
                        Log(Debug) << "迭代次数为：" << count << '\n'
                                   << "未找到收敛的信号传播时间" << '\n';
                        break;
                    }

                    signalPropagationTime = newsignalPropagationTime;
                    count++;
                }

                // 找到了信号传播时间之后，确定了卫星坐标，开始填充矩阵
                double second = curTime->SecondFromRefTime() - signalPropagationTime;
                auto _signalPostTime = TimeFactory::MakeTimePtr(TimeFactory::ValueType::UnixTimeSys)->GetRefTime()->TimeAfterSecond(second);
                auto signalPostTime = std::shared_ptr<TimeSys>(_signalPostTime);

                NavCalculateResult navCal(mesData, signalPostTime);
                auto _rsPosition = navCal.GetPosition();
                auto rsPosition = std::dynamic_pointer_cast<CartesianCRD>(_rsPosition);

                auto rrPosition = std::dynamic_pointer_cast<CartesianCRD>(reciverPosition);

                double distance = DistanceOfCoordinate(rrPosition, rsPosition);
                double l = (rrPosition->GetX() - rsPosition->GetX()) / distance;

                double m = (rrPosition->GetY() - rsPosition->GetY()) / distance;

                double n = (rrPosition->GetZ() - rsPosition->GetZ()) / distance;

                double Pij = 0;
                if (mesRecord->gnssType == GPS)
                {
                    Pij = -BandUtil::GetLambda(GPS, 1) * doppler;
                }
                else if (mesRecord->gnssType == BDS)
                {
                    Pij = -BandUtil::GetLambda(BDS, 2) * doppler;
                }

                auto rsVelocity = navCal.GetVelocity();
                double w = Pij + l * rsVelocity[0] + m * rsVelocity[1] + n * rsVelocity[2] - Mathf::c * navCal.GetClockSpeed();

                auto tmp = CRDFactory::MakeCRDPtr(CRDFactory::ValueType::TopoCentricPolarCRDSys, *rrPosition);

                CRDFactory::SwitchTo(CRDFactory::ValueType::TopoCentricPolarCRDSys, rsPosition, &tmp, rrPosition);
                std::shared_ptr<TopoCentricPolarCRD> ans = std::dynamic_pointer_cast<TopoCentricPolarCRD>(tmp);
                double p = Mathf::Sin(ans->GetElevation()) * Mathf::Sin(ans->GetElevation());

                B[i][0] = l;
                B[i][1] = m;
                B[i][2] = n;
                B[i][3] = 1;

                W[i][0] = w;

                P[i][i] = p;
            }

            Matrix x = Matrixf::InverseMatrix(Matrixf::TransposeMatrix(B) * P * B) * Matrixf::TransposeMatrix(B) * P * W;
            x.Normalize();

            std::vector<double> ans(3);
            ans[0] = x[0][0];
            ans[1] = x[1][0];
            ans[2] = x[2][0];

            Matrix V = B * x - W;
            double sigma = Mathf::Sqrt((Matrixf::TransposeMatrix(V) * P * V)[0][0] / (obsDataSize - 4));
            Matrix D = Matrixf::MulMatrix(sigma, Matrixf::InverseMatrix(Matrixf::TransposeMatrix(B) * P * B));
            D.Normalize();
            if (DOP != nullptr)
                *DOP = D;

            *output = ans;
            return true;
        }

    private:
        static double DistanceOfCoordinate(const CRDFactory::CRDPtr &c1, const CRDFactory::CRDPtr &c2)
        {
            double ans = 0;
            auto Cc1 = std::dynamic_pointer_cast<CartesianCRD>(c1);
            auto Cc2 = std::dynamic_pointer_cast<CartesianCRD>(c2);

            ans = Mathf::Sqrt((Cc1->GetX() - Cc2->GetX()) * (Cc1->GetX() - Cc2->GetX()) +
                              (Cc1->GetY() - Cc2->GetY()) * (Cc1->GetY() - Cc2->GetY()) +
                              (Cc1->GetZ() - Cc2->GetZ()) * (Cc1->GetZ() - Cc2->GetZ()));

            return ans;
        }
    };
}